import rclpy
from rclpy.node import Node
from geometry_msgs.msg import TransformStamped
from tf2_ros import TransformBroadcaster
import math

class TF2Broadcaster(Node):
    def __init__(self):
        super().__init__('tf2_broadcaster')
        self.tf_broadcaster = TransformBroadcaster(self)
        self.timer = self.create_timer(0.1, self.broadcast_transform)

    def broadcast_transform(self):
        # 创建 TransformStamped 消息
        t = TransformStamped()

        # 填充消息内容
        t.header.stamp = self.get_clock().now().to_msg()
        t.header.frame_id = 'parent_frame'
        t.child_frame_id = 'child_frame'
        t.transform.translation.x = 1.0
        t.transform.translation.y = 2.0
        t.transform.translation.z = 0.0

        # 使用正弦函数动态改变 z 轴角度
        angle = math.radians((self.get_clock().now().seconds_nanoseconds()[0] * 10) % 360)
        t.transform.rotation.x = 0.0
        t.transform.rotation.y = 0.0
        t.transform.rotation.z = math.sin(angle / 2.0)
        t.transform.rotation.w = math.cos(angle / 2.0)

        # 广播变换
        self.tf_broadcaster.sendTransform(t)
        self.get_logger().info('Broadcasted Transform')

def main(args=None):
    rclpy.init(args=args)
    node = TF2Broadcaster()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()
